#ifndef __SIM3SOLVER_H__
#define __SIM3SOLVER_H__
#include "../include/Typedef.hpp"
#include <vector>
#include "../include/Frame.hpp"
using namespace std;
namespace ORBSLAM
{
    class Sim3Solver
    {
    private:
        /* data */
        int mn_iteration;

        bool mb_fix_scale;

        KeyFrame *mp_keyframe1;
        KeyFrame *mp_keyframe2;

        int mn_match_num;

        vector<MapPoint *> mvp_mpt1;
        vector<MapPoint *> mvp_mpt2;
        vector<MapPoint *> mvp_matched12;
        vector<int> mvn_indices1;
        vector<Mat> mvmat_x3dc1;
        vector<Mat> mvmat_x3dc2;
        vector<Mat> mmat_p1im1;
        vector<Mat> mmat_p2im2;

        vector<int> mvn_all_indices;

        vector<float> mvf_maxerror1;
        vector<float> mvf_maxerror2;

        double md_ransac_prob;
        int mn_ransac_min_inliner;
        int mn_ransac_max_its;

        int mn_mpt_num;

        Mat mmat_r12i;
        Mat mmat_t12i;
        Mat mmat_T12i;
        Mat mmat_T21i;
        float mf_s12i;
        int mn_inlineri;
        vector<bool> mvb_inlineri;

        Mat mmat_best_T12;
        Mat mmat_best_rotation;
        Mat mmat_best_translation;
        float mf_best_scale;
        int mn_best_inliner;
        vector<bool> mvb_best_inliner;

    public:
        Sim3Solver(/* args */);
        ~Sim3Solver();
        Sim3Solver(KeyFrame *p_keyframe1, KeyFrame *p_keyframe2, const vector<MapPoint *> &vp_matched12, const bool b_fix_scale = true, vector<KeyFrame *> vp_matched_mpt_keyframe = vector<KeyFrame *>());
        void Set_parameters(double probability = 0.99, int n_inliner = 6, int n_max_iteration = 300);
        Mat Find(vector<bool> &vb_inliner12, int &n_inliner);
        Mat Iterate(int n_iteration, bool &b_nomore, vector<bool> &vb_inliner, int &n_inliner);
        Mat Iterate(int n_ietration, bool &b_nomore, vector<bool> &vb_inliner, int &n_inliner, bool &b_coverge);

        Mat Get_estimated_rotation();
        Mat Get_estimated_translation();
        float Get_estimated_scale();

    protected:
        void Compute_centroid(Mat &P, Mat &Pr, Mat &C);
        void Compute_sim3(Mat &P1, Mat &P2);
        void Check_inline();
        void Project(const vector<Mat> &vp3dw,vector<Mat> &vp2d,Mat Tcw);
        void From_camera_to_image(const vector<Mat> &vp3dc,vector<Mat> &vp2d);
    };

}
#endif // __SIM3SOLVER_H__